import paho.mqtt.client as mqtt
import ssl
import base64
import asn1tools
import json
import time
import threading
import random
import datetime

start_timestamp = int(datetime.datetime(2004,1,1).timestamp()*1000)
def on_connect(client, userdata, flags, rc):
    print("Connected with result code "+str(rc))

def on_message(client, userdata, msg):
    print(msg.topic+" "+str(msg.payload))

client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
client.tls_set(ca_certs="cert/root.crt", certfile="cert/vehicle1.crt", keyfile="cert/vehicle1_key.pem", 
    cert_reqs=ssl.CERT_REQUIRED, tls_version=ssl.PROTOCOL_TLSv1_2, ciphers=None)

client.connect("broker", 8883, 60)

# Define the message to publish
cam_asn = asn1tools.compile_files('CAM.asn', 'uper')
cam_json = {
    'header': {
        'protocolVersion': 1,
        'messageID': 2,
        'stationID': 123
    },
    'cam': {
        'generationDeltaTime': 123,
        'camParameters': {
            'basicContainer': {
                'stationType': 0,
                'referencePosition': {
                    'latitude': 12345678,
                    'longitude': 87654321,
                    'positionConfidenceEllipse': {
                        'semiMajorConfidence': 4095,
                        'semiMinorConfidence': 4095,
                        'semiMajorOrientation': 3601
                    },
                    'altitude': {
                        'altitudeValue': 800001,
                        'altitudeConfidence': 'unavailable'
                    }
                }
            },
            'highFrequencyContainer': (
                'basicVehicleContainerHighFrequency', {
                    'heading': {
                        'headingValue': 3601,
                        'headingConfidence': 127
                    },
                    'speed': {
                        'speedValue': 30,
                        'speedConfidence': 127
                    },
                    'driveDirection': 'unavailable',
                    'vehicleLength': {
                        'vehicleLengthValue': 1023,
                        'vehicleLengthConfidenceIndication': 'unavailable'
                    },
                    'vehicleWidth': 62,
                    'longitudinalAcceleration': {
                        'longitudinalAccelerationValue': 161,
                        'longitudinalAccelerationConfidence': 102
                    },
                    'curvature': {
                        'curvatureValue': 1023,
                        'curvatureConfidence': 'unavailable'
                    },
                    'curvatureCalculationMode': 'unavailable',
                    'yawRate': {
                        'yawRateValue': 32767,
                        'yawRateConfidence': 'unavailable'
                    }
                }
            )
        }
    }
}
denm_asn = asn1tools.compile_files('DENM.asn', 'uper')
denm_json = {
    'header': {
        'protocolVersion': 1,
        'messageID': 2,
        'stationID': 123
    },
    'denm': {
        'management': {
            'actionID': {
                'originatingStationID': 123,
                'sequenceNumber': 1
            },
            'detectionTime': 1,
            'referenceTime': 1,
            'eventPosition': {
                'latitude': 123,
                'longitude': 321,
                'positionConfidenceEllipse': {
                    'semiMajorConfidence': 4095,
                    'semiMinorConfidence': 4095,
                    'semiMajorOrientation': 3601
                },
                'altitude': {
                    'altitudeValue': 800001,
                    'altitudeConfidence': 'unavailable'
                }
            },
            'validityDuration': 20,
            'stationType': 0
        },
        'situation': {
            'informationQuality': 0,
            'eventType': {
                'causeCode': 99,
                'subCauseCode': 1 
            },
            'linkedCause': {
                'causeCode': 99,
                'subCauseCode': 1
            }
        }
    }
}

# Read the simulation location list
roads_csv = open('roads.csv', 'r')
lines = roads_csv.readlines()
locations = []
for i in range(1, len(lines)):
    record = lines[i].split(',')
    lat = int(float(record[1])*10000000)
    lon = int(float(record[2])*10000000)
    locations.append((lat, lon))

def publish_data(VIN, denm_flag=False):
    for i in range(1):
        rand_denm = random.randint(0, len(locations))
        for j in range(len(locations)):
            cam_json['cam']['camParameters']['basicContainer']['referencePosition']['latitude'] = locations[j][0]
            cam_json['cam']['camParameters']['basicContainer']['referencePosition']['longitude'] = locations[j][1]
            cam_encoded = cam_asn.encode('CAM', cam_json)
            cam_b64 = str(base64.b64encode(cam_encoded),'utf-8')
            client.publish("vehicle/cam/"+VIN,cam_b64)
            if denm_flag:
                if j == rand_denm:
                    denm_json['denm']['management']['actionID']['sequenceNumber'] = i
                    denm_json['denm']['management']['eventPosition']['latitude'] = locations[j][0]
                    denm_json['denm']['management']['eventPosition']['longitude'] = locations[j][1]
                    denm_json['denm']['situation']['eventType']['causeCode'] = 99
                    denm_json['denm']['situation']['eventType']['subCauseCode'] = 1
                    #detect_timestamp = int(datetime.datetime.utcnow().timestamp()*1000) - start_timestamp
                    detect_timestamp = int(datetime.datetime.now(datetime.timezone.utc).timestamp()*1000) - start_timestamp
                    denm_json['denm']['management']['detectionTime'] = detect_timestamp
                    denm_json['denm']['management']['referenceTime'] = detect_timestamp
                    denm_encoded = denm_asn.encode('DENM', denm_json)
                    denm_b64 = str(base64.b64encode(denm_encoded),'utf-8')
                    client.publish("vehicle/denm/"+VIN,denm_b64)
            time.sleep(0.5)
        time.sleep(10)

def subscribe_data(VIN):
    client.subscribe("vehicle/"+VIN+"/denm")
    
class myThread (threading.Thread):
    def __init__(self, VIN, denm_flag):
        threading.Thread.__init__(self)
        self.VIN = VIN
        self.denm_flag = denm_flag
    def run(self):
        publish_data(self.VIN, self.denm_flag)
        subscribe_data(self.VIN)

thread1 = myThread("Vehicle1", True)
thread2 = myThread("Vehicle2", False)

thread1.start()
time.sleep(1)
thread2.start()
thread1.join()
thread2.join()

client.disconnect()

'''
road_json_f = open('road.json', 'r')
lines = road_json_f.readlines()
result = ''
for l in lines:
    result += l.strip()
road_json_f.close()
road_json = json.loads(result)
locations = road_json['snappedPoints']
points = ['points,latitude,longitude']
i = 1
for loc in locations:
    points.append(str(i)+','+str(loc['location']['latitude'])+','+str(loc['location']['longitude']))
roads_csv = open('roads.csv', 'w')
roads_csv.writelines('\n'.join(points))
roads_csv.close()
'''
